//////////////////////////////////////////////////////////////////////////
/// Copyright (c) 2011, Sidorenko 'PC' Alexander                       ///
/// mailto: sidorenko /*dot*/ alexander (at) gmail [dot] com           ///
/// This file is a part of the Radiating Gas Dynamics project          ///
//////////////////////////////////////////////////////////////////////////

#ifndef RiemannSolver_INCLUDED
#define RiemannSolver_INCLUDED

template<unsigned int N>
class RiemannSolver
{
public:

	typedef typename VecN<N> Vec;

	RiemannSolver(gdScalar gamma);
	~RiemannSolver();

	inline gdScalar getTolerance() const { return m_tolerance; }
	inline void setTolerance(gdScalar tolerance) { m_tolerance = tolerance; }
	inline void setThreshold(unsigned int threshold) { m_threshold = threshold; }

	//void state(gdScalar x, gdScalar r1, gdScalar p1, gdScalar v1,
	//					   gdScalar r2, gdScalar p2, gdScalar v2,
	//					   gdScalar &r, gdScalar &p, gdScalar &v) const;

	void interfaceState(const Vec& n,
						gdScalar r1, gdScalar a1, gdScalar p1, const Vec& v1,
						gdScalar r2, gdScalar a2, gdScalar p2, const Vec& v2,
						gdScalar &r, gdScalar &a, gdScalar &p, gdScalar& vn, Vec& vt) const;

private:

	gdScalar density(gdScalar r0, gdScalar p0, gdScalar p1) const;

	void wave(gdScalar x, gdScalar r0, gdScalar a0, gdScalar p0, gdScalar v0,
						  gdScalar r1, gdScalar a1, gdScalar p1, gdScalar v1,
						  gdScalar& r, gdScalar& a, gdScalar& p, gdScalar& v) const;

	void velocity_1jet(gdScalar r0, gdScalar a0, gdScalar p0, gdScalar v0, gdScalar p1,
					   gdScalar &v1, gdScalar &v1prime) const;

	void newtonRaphson(gdScalar r1, gdScalar a1, gdScalar p1, gdScalar v1,
					   gdScalar r2, gdScalar a2, gdScalar p2, gdScalar v2,
					   gdScalar &p, gdScalar &v) const;

	gdScalar   m_gamma
			 , m_gamma0
			 , m_gamma1
			 , m_gamma2
			 , m_gamma3
			 , m_gamma4
			 , m_gamma5;

	gdScalar m_tolerance;

	unsigned int m_threshold;

};

// Implementation
template<unsigned int N>
RiemannSolver<N>::RiemannSolver(gdScalar gamma)
: m_tolerance(gdScalar(1e-6))
, m_gamma(gamma)
, m_threshold(10)
{
	//m_gamma0 = gdScalar(1.0) / m_gamma;
	//m_gamma1 = gdScalar(0.5) * (m_gamma + gdScalar(1.0));
	//m_gamma2 = gdScalar(0.5) * (m_gamma - gdScalar(1.0));
	//m_gamma3 = m_gamma2 / m_gamma1;
	//m_gamma4 = gdScalar(1.0) / m_gamma2;
	//m_gamma5 = m_gamma2 * m_gamma0;
	m_gamma0 = 1.0 / m_gamma;
	m_gamma1 = 0.5 * (m_gamma + 1.0);
	m_gamma2 = 0.5 * (m_gamma - 1.0);
	m_gamma3 = m_gamma2 / m_gamma1;
	m_gamma4 = 1.0 / m_gamma2;
	m_gamma5 = m_gamma2 * m_gamma0;
}

template<unsigned int N>
RiemannSolver<N>::~RiemannSolver()
{

}

template<unsigned int N>
gdScalar RiemannSolver<N>::density(gdScalar r0, gdScalar p0, gdScalar p1) const
{
	if (p1 >= p0)
		return r0 * (p1 + m_gamma3 * p0) / (m_gamma3 * p1 + p0);
	else
	if (p1 > gdScalar(0.0))
		return r0 * pow(p1 / p0, m_gamma0);
	else
		return gdScalar(0.0);
}

template<unsigned int N>
void RiemannSolver<N>::wave(gdScalar x, gdScalar r0, gdScalar a0, gdScalar p0, gdScalar v0,
							gdScalar r1, gdScalar a1, gdScalar p1, gdScalar v1,
							gdScalar& r, gdScalar& a, gdScalar& p, gdScalar& v) const
{
	if (p1 > p0)
	{
		gdScalar s = (r1 * v1 - r0 * v0) / (r1 - r0);
		if (abs(x - s) < m_tolerance) // x == s
		{
			r = gdScalar(0.5) * (r0 + r1);
			p = gdScalar(0.5) * (p0 + p1);
			v = gdScalar(0.5) * (v0 + v1);
			a = sqrt(m_gamma * p / r);
		}
		else
		if (x > s)
		{
			r = r0;
			a = a0;
			p = p0;
			v = v0;
		}
		else
		{ // x < s
			r = r1;
			a = a1;
			p = p1;
			v = v1;
		}
	}
	else
	{
		if (x >= v0 + a0)
		{
			r = r0;
			a = a0;
			p = p0;
			v = v0;
		}
		else
		if (x <= v1 + a1)
		{
			r = r1;
			a = a1;
			p = p1;
			v = v1;
		}
		else
		{
			a = a0 - m_gamma3 * (v0 + a0 - x);
			r = r0 * pow(a / a0, m_gamma4);
			p = p0 * pow(r / r0, m_gamma);
			v = x - a;
		}
	}
}


template<unsigned int N>
void RiemannSolver<N>::velocity_1jet(gdScalar r0, gdScalar a0, gdScalar p0, gdScalar v0, gdScalar p1,
									 gdScalar &v1, gdScalar &v1prime) const
{
	gdScalar t1, t2;
	if (p1 >= p0)
	{
		t1 = m_gamma1 * p1 + m_gamma2 * p0;
		t2 = sqrt(r0 * t1);
		v1 = v0 + (p1 - p0) / t2;
		v1prime = (gdScalar(1.0) - gdScalar(0.5) * m_gamma1 * (p1 - p0) / t1) / t2;
	}
	else
	if (p1 > gdScalar(0.0))
	{
		t1 = pow(p1 / p0, m_gamma5);
		v1 = v0 + a0 * (t1 - gdScalar(1.0)) * m_gamma4;
		v1prime = m_gamma0 * a0 * t1 / p1;
	}
	else
	{
		v1 = v0 - m_gamma4 * a0;
		v1prime = FLT_MAX;
	}
}

template<unsigned int N>
void RiemannSolver<N>::newtonRaphson(gdScalar r1, gdScalar a1, gdScalar p1, gdScalar v1,
									 gdScalar r2, gdScalar a2, gdScalar p2, gdScalar v2,
									 gdScalar &p, gdScalar &v) const
{
	gdScalar w1, w2;
	w1 = gdScalar(1.0) / (r1 * a1);
	w2 = gdScalar(1.0) / (r2 * a2);
	p = max((w1 * p1 + w2 * p2 + v1 - v2) / (w1 + w2), m_tolerance);
	gdScalar e = 1.0 + m_tolerance;
	gdScalar u, uprime, vprime;
	for (unsigned int i = 0; i < m_threshold; ++i)
	{
		velocity_1jet(r1, a1, p1, -v1, p, u, uprime);
		velocity_1jet(r2, a2, p2, v2, p, v, vprime);
		e = (v + u) / (vprime + uprime);
		p = max(p - e, m_tolerance);
		e = abs(e / p);

		if (e <= m_tolerance)
		{
			break;
		}
	}
	v = gdScalar(0.5) * (v - u);
}

//template<unsigned int N>
//void RiemannSolver<N>::state(gdScalar x, gdScalar r1, gdScalar p1, gdScalar v1,
//							 gdScalar r2, gdScalar p2, gdScalar v2,
//							 gdScalar &r, gdScalar &p, gdScalar &v) const
//{
//	// TODO:
//	//a1 = sqrt(gamma*p1/r1);
//	//a2 = sqrt(gamma*p2/r2);
//	//[p0,v0] = newton_raphson(r1,a1,p1,v1,r2,a2,p2,v2);
//	//r = zeros(size(x));
//	//p = zeros(size(x));
//	//v = zeros(size(x));
//	//for i = 1:numel(x)
//	//	if x(i) < v0
//	//		r0 = density(r1,p1,p0);
//	//		a0 = sqrt(gamma*p0/r0);
//	//		[r(i),a,p(i),v(i)] = wave(-x(i),r1,a1,p1,-v1,r0,a0,p0,-v0);
//	//		v(i) = -v(i);
//	//	elseif x(i) > v0
//	//		r0 = density(r2,p2,p0);
//	//		a0 = sqrt(gamma*p0/r0);
//	//		[r(i),a,p(i),v(i)] = wave(x(i),r2,a2,p2,v2,r0,a0,p0,v0);
//	//	else
//	//		r0 = density(r1,p1,p0);
//	//		a0 = sqrt(gamma*p0/r0);
//	//		[rr1,a,pp1,vv1] = wave(-x(i),r1,a1,p1,-v1,r0,a0,p0,-v0);
//	//		vv1 = -vv1;
//	//		r0 = density(r2,p2,p0);
//	//		a0 = sqrt(gamma*p0/r0);
//	//		[rr2,a,pp2,vv2] = wave(x(i),r2,a2,p2,v2,r0,a0,p0,v0);
//	//		r(i) = 0.5*(rr1+rr2);
//	//		p(i) = 0.5*(pp1+pp2);
//	//		v(i) = 0.5*(vv1+vv2);
//	//	end
//	//end
//}

template<unsigned int N>
void RiemannSolver<N>::interfaceState(const Vec& n,
									  gdScalar r1, gdScalar a1, gdScalar p1, const Vec& v1,
									  gdScalar r2, gdScalar a2, gdScalar p2, const Vec& v2,
									  gdScalar &r, gdScalar &a, gdScalar &p, gdScalar& vn, Vec& vt) const
{
	gdScalar v1n, v2n;
	Vec v1t, v2t;
	v1n = v1.dotProduct(n);
	v2n = v2.dotProduct(n);
	v1t = v1 - n * v1n;
	v2t = v2 - n * v2n;
	gdScalar p0, v0, a0, r0;
	newtonRaphson(r1, a1, p1, v1n,
				  r2, a2, p2, v2n,
				  p0, v0);
	if (abs(v0) < m_tolerance) // (v0 == 0)
	{
		r0 = density(r1,p1,p0);
		a0 = sqrt(m_gamma * p0 / r0);
		gdScalar rr1, aa1, pp1, vn1,
				 rr2, aa2, pp2, vn2;
		wave(0.0, r1, a1, p1, -v1n, r0, a0, p0,-v0, rr1, aa1, pp1, vn1);
		vn1 = -vn1;
		r0 = density(r2,p2,p0);
		a0 = sqrt(m_gamma * p0 / r0);
		wave(0.0, r2, a2, p2, v2n, r0, a0, p0, v0, rr2, aa2, pp2, vn2);
		r = gdScalar(0.5) * (rr1 + rr2);
		p = gdScalar(0.5) * (pp1 + pp2);
		vn = gdScalar(0.5) * (vn1 + vn2);
		vt = (v1t + v2t) * gdScalar(0.5);
		a = sqrt(m_gamma * p / r);
	}
	else
	if (v0 > 0)
	{
		r0 = density(r1, p1, p0);
		a0 = sqrt(m_gamma * p0 / r0);
		wave(0.0, r1, a1, p1, -v1n, r0, a0, p0, -v0, r, a, p, vn);
		vn = -vn;
		vt = v1t;
	}
	else // (v0 < 0)
	{
		r0 = density(r2, p2, p0);
		a0 = sqrt(m_gamma *p0 / r0);
		wave(0.0, r2, a2, p2, v2n, r0, a0, p0, v0, r, a, p, vn);
		vt = v2t;
	}
}

#endif // RiemannSolver_INCLUDED